#!/usr/bin/env python
import rospy

from std_msgs.msg import String
from magabot.msg import Status
from magabot.msg import Actuation

turn = -9 
check = [False,False,False,False]
action = ''

# ------------------------------------------------------------------
def process_sensing(stat):		# il valore degli ir e' ora in stat.ir

	global turn, check
	
	check = stat.bumpers
	
	print "+ (left: %d; center: %d; right: %d) " % (stat.ir[0], stat.ir[1], stat.ir[2] )
		
	
	turn = 0

	if ( (stat.ir[1] > 100) ):		# destra
		turn = 1
		
	if ( (stat.ir[0] > 100) ):		# sinistra
		turn = -1

	if ( (stat.ir[1] > 100) and (stat.ir[0] > 100)  ):		# stop
		turn = 2
	
	aaa()
	
	
def speech(command):
	global action
	if command.data == 'here':
		action = 'stop'
	elif command.data == 'home':
		action = 'go'
		
	print "action:", action
# ------------------------------------------------------------------

def run():

	rospy.init_node('magabot_line_follower')		# nome del nodo

	rospy.Subscriber("magabot/status", Status, process_sensing)		# leggo un messaggio di tipo Status dal topic
	rospy.Subscriber("recognizer/output", String, speech)
	pub = rospy.Publisher('magabot/actuation', Actuation)			# pubblico un messaggio di tipo Actuation sul topic

	pub.publish(msg)
	
	rospy.spin()


def aaa():
	global turn, check
	
	if action == 'stop':
		msg.velocity[0] = 0
		msg.velocity[1] = 0
		
		pub.publish(msg)	
	
	else:

		print " turn = ", turn
		if ( turn == 0 ):		# vai dritto
			msg.velocity[0] = 8
			msg.velocity[1] = 8

		elif ( turn == 1 ):		# gira a destra
			msg.velocity[0] = 9
			msg.velocity[1] = -9


		elif ( turn == -1 ):		# gira a sinistra
			msg.velocity[0] = -9
			msg.velocity[1] = 9			
		
		if (check[0] == True) or (check[1] == True) or (check[2] == True) or (check[3] == True) or turn == 2:

			msg.velocity[0] = 0
			msg.velocity[1] = 0
			
			pub.publish(msg)	

			print('stopping')
			exit()
			
				
		pub.publish(msg)
			
	

# ------------------------------------------------------------------

	        
if __name__ == '__main__':

	msg = Actuation()		# msg e' un oggetto di tipo Actuation()

	# Inizializzazione	
	msg.set_velocity = True
	msg.velocity[0] = 0
	msg.velocity[1] = 0
	msg.set_leds = False
	msg.red = 0
	msg.green = 0
	msg.blue = 0
	
	print " Press a button..."
	raw_input()


	run()
	
		







   
